//Realeased under really Creative Commons! 
//This just a basic demo code...
//Reads and pulses the 8 channels... 
//By Jordi Munoz

//**************Compas
#include <avr/interrupt.h>
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Compass.h> // Compass Library
#include <AP_Math.h>		// ArduPilot Mega Vector/Matrix math Library
#include <Wire.h>

FastSerialPort0(Serial);

#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi

AP_Compass_HMC5843 compass;

//**************RangeFinder
#include <AP_RangeFinder.h>     // Range finder library
#include <AP_ADC.h>		// ArduPilot Mega Analog to Digital Converter Library

#define RF_PIN AP_RANGEFINDER_PITOT_TUBE // the pitot tube on the front of the oilpan
//#define RF_PIN A5 // A5 is the far back-right pin on the oilpan (near the CLI switch)

// declare global instances for reading pitot tube
AP_ADC_ADS7844	adc;

// create the range finder object
//AP_RangeFinder_SharpGP2Y aRF;
AP_RangeFinder_MaxsonarXL aRF;

//**************Other
volatile unsigned int Start_Pulse =0;
volatile unsigned int Stop_Pulse =0;
volatile unsigned int Pulse_Width =0;

volatile int Test=0;
volatile int Test2=0;
volatile int Temp=0;
volatile int Counter=0;
volatile byte PPM_Counter=0;
volatile int PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400};
int All_PWM=1500;
boolean manned;


long timer=0;
long timer2=0;

void setup()
{
  Init_PWM1(); //OUT2&3 
  Init_PWM3(); //OUT6&7
  Init_PWM5(); //OUT0&1
  Init_PPM_PWM4(); //OUT4&5
  Serial.begin(57600);
  manned = false;
  
  //Compas
  Wire.begin();
  compass.init();
  
  compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD);  // set compass's orientation on aircraft.
  compass.set_offsets(0,0,0);  // set offsets to account for surrounding interference
  compass.set_declination(ToRad(0.0));  // set local difference between magnetic north and true north


  //RangeFinder
  adc.Init();            // APM ADC library initialization
  aRF.init(RF_PIN, &adc);
}

void loop()
{
  
   if(InputCh(4) > 1500) {
     if(manned){
       manned = false;
       //Записать направление магнитометра
       //compass.mag_x
     }
   } else {
      manned = true; 
   }
   //Switch low, forward the PPM
   if(manned) //Reading channel 5 to change mode
   {
     OutputCh(0, InputCh(0));
     OutputCh(1, InputCh(1));
     OutputCh(2, InputCh(2));
     //OutputCh(3, InputCh(3));
   }
   else
   {
     if(aRF.read()<300) {
       turnDirig(); //Поворачиваем
     }
     //Выравнивание по координатам
   }
   delay(20);
}

void turnDirig() {
  
}


